//
// Created by xujingyi on 2021/5/21.
//

#ifndef ALLIANCE_SENTRY_V2_0_SMOOTHKF_H
#define ALLIANCE_SENTRY_V2_0_SMOOTHKF_H
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include "opencv2/video/tracking.hpp"
namespace rm{
class SmoothKF{
public:
    SmoothKF()=default;
    ~SmoothKF()=default;
    SmoothKF(const int mode=0,int state_num=-1,int measure_num=-1,const float &proportion=-1);
    void init();
    float renew(const float &num);
    cv::Point2f renew(const cv::Point2f &point2f);
    cv::Point3f renew(const cv::Point3f &point3f);

private:
    cv::KalmanFilter *Kf;
    int _state_num=-1,_measure_num=-1;
    float _smooth_proportion=-1;
    float _mode;//1:coordination
    cv::Mat _measurement1 = cv::Mat::zeros(1, 1, CV_32FC1);
    cv::Mat _measurement2 = cv::Mat::zeros(2, 1, CV_32FC1);
    cv::Mat _measurement3 = cv::Mat::zeros(3, 1, CV_32FC1);
};

class Smoother{
public:
    Smoother (){
        KFc=new SmoothKF(1);
        KFPixel=new SmoothKF(2);
    };
    void init(){
        KFc->init();
        KFPixel->init();
    }
    ~Smoother()= default;

    SmoothKF *KFc;
    SmoothKF *KFPixel;
};
};
#endif //ALLIANCE_SENTRY_V2_0_SMOOTHKF_H
